/*****************************************************************************/
/**
 *  @file   Frustum.cpp
 *  @author Naohisa Sakamoto
 */
/*----------------------------------------------------------------------------
 *
 *  Copyright (c) Visualization Laboratory, Kyoto University.
 *  All rights reserved.
 *  See http://www.viz.media.kyoto-u.ac.jp/kvs/copyright/ for details.
 *
 *  $Id$
 */
/*****************************************************************************/
#include "Frustum.h"
#include <kvs/OpenGL>
#include <kvs/Matrix44>
#include <kvs/ObjectManager>
#include <kvs/ObjectBase>
#include <kvs/Scene>
#include <kvs/Camera>


namespace
{

inline float Distance( const kvs::Vec4& plane, const kvs::Vec3& point )
{
    return plane[0] * point.x() + plane[1] * point.y() + plane[2] * point.z() + plane[3];
}

inline kvs::Vec4 Normalize( const kvs::Vec4& v )
{
    const float l = kvs::Vec3( v.x(), v.y(), v.z() ).length();
    return v / l;
}

}


Frustum::Frustum( kvs::Scene* scene )
{
    kvs::OpenGL::WithPushedMatrix p0( GL_MODELVIEW );
    p0.loadIdentity();
    {
        scene->camera()->update();
        kvs::OpenGL::PushMatrix();
        {
            // Apply initial translation and scaling information
            // for the object to OpenGL matrix stack.
            scene->updateGLModelingMatrix( scene->objectManager()->object(1) );

            // Calculate modelview-projection matrix.
            const kvs::Mat4 M = kvs::OpenGL::ModelViewMatrix();
            const kvs::Mat4 P = kvs::OpenGL::ProjectionMatrix();
            const kvs::Mat4 PM = P * M;

            // Caluclate six planes of the view frustum.
            m_planes[0] = ::Normalize( PM[3] + PM[0] ); // left
            m_planes[1] = ::Normalize( PM[3] - PM[0] ); // right
            m_planes[2] = ::Normalize( PM[3] + PM[1] ); // bottom
            m_planes[3] = ::Normalize( PM[3] - PM[1] ); // top
            m_planes[4] = ::Normalize( PM[3] + PM[2] ); // near
            m_planes[5] = ::Normalize( PM[3] - PM[2] ); // far
        }
        kvs::OpenGL::PopMatrix();
    }
}

bool Frustum::include( const kvs::Vec3& min, const kvs::Vec3& max ) const
{
    // Cornner points of the box.
    const kvs::Vec3 point[8] = {
        kvs::Vec3( min.x(), min.y(), min.z() ),
        kvs::Vec3( max.x(), min.y(), min.z() ),
        kvs::Vec3( max.x(), max.y(), min.z() ),
        kvs::Vec3( min.x(), max.y(), min.z() ),
        kvs::Vec3( min.x(), min.y(), max.z() ),
        kvs::Vec3( max.x(), min.y(), max.z() ),
        kvs::Vec3( max.x(), max.y(), max.z() ),
        kvs::Vec3( min.x(), max.y(), max.z() ) };

    for ( size_t i = 0; i < 6; i++ )
    {
        const kvs::Vec4& plane = m_planes[i];
        if ( ::Distance( plane, point[0] ) > 0 ) continue;
        if ( ::Distance( plane, point[1] ) > 0 ) continue;
        if ( ::Distance( plane, point[2] ) > 0 ) continue;
        if ( ::Distance( plane, point[3] ) > 0 ) continue;
        if ( ::Distance( plane, point[4] ) > 0 ) continue;
        if ( ::Distance( plane, point[5] ) > 0 ) continue;
        if ( ::Distance( plane, point[6] ) > 0 ) continue;
        if ( ::Distance( plane, point[7] ) > 0 ) continue;
        return false;
    }

    return true;
}
